
/*
 * Mainloop.java
 *
 * Created on May 30, 2008, 11:16 PM
 *
 * This is the primary execution environment, from which all
 * robot action is initiated via the Actors framework.
 * Mainloop must remaining running; stop() will cause
 * StartApplication.notifyDestroyed() to be called, resulting in the resetting of
 * the SPOT.
 * Once initialized, Mainloop execution follows a specific pattern,
 * as specified under step().
 * Mainloop keeps track of all actuating agents(Actors) as well
 * as the robot platform itself.
 */
package org.gunncs.daemons;

import com.sun.spot.sensorboard.peripheral.ISwitch;
import org.gunncs.actors.*;
import java.io.*;
import com.iaroc.irobot.*;
import org.gunncs.actoriface.*;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import java.util.*;
import com.sun.spot.sensorboard.io.*;
import com.sun.spot.sensorboard.peripheral.ISwitchListener;;

/**
 *
 * @author Anandg, ajc
 */
public class Mainloop extends Thread implements ISwitchListener {

    public static final int VELOCITY = 100;//150;
    boolean running;
    boolean stepping;
    IIOPin rxPin;    //Corresponds to rx on the sensor devices, tx(digital) on the SPOT
    private Robot bot;
    Vector actors;
    private ISwitch master;
    private ISwitch step;
    private static Mainloop instance;
    ITriColorLED overallStatus = EDemoBoard.getInstance().getLEDs()[1];

    public Mainloop(Robot b) {
        this(b, null);
    }

    public Mainloop(Robot b, IIOPin rx) {
        running = false;
        stepping = false;
        bot = b;
        rxPin = rx;
        instance = this;
        initAdvancedControl();
    }

    public static Mainloop getInstance() {
        return instance;
    }

    /**
     * init() initializes the robot with all of its hardware, being the intrinsic
     * iRobot Create itself, as well as any additional sensors. This should
     * only be called once per application cycle.
     * The order in which sensors are added determines their indexing in all
     * getSensors() calls. see Robot.getSensors()
     * The addition of any sensors to the hardware platform should require only
     * modification of this method with the appropriate addSensor() calls.
     * The addition of any actuators should be placed here, as should
     * any actuating agents(Actors)
     */
    public void init() {
        //LED status ORANGE = UNINITIALIZED
        overallStatus.setColor(LEDColor.ORANGE);
        overallStatus.setOn();


        //Robot specific hardware specified
		    /*
         * <GunnCS '09 Robot Configuration>
         * [0] Stock BumpSensors, left and right ->
         * 	      state acquired over Create serial line
         * [1] 1x Forward Rangefinder(Maxbotix EZ3) ->
         *        Analog output read at A0, 3V supply
         * [2,4] 2x Outward facing Rangefinders(Maxbotix EZ3) ->
         *		  Analog output read at A1 & 2, 3V
         * [3,5] 2x Servos, actuating outward rangefinders[1,3] ->
         *        Controlled via H0 & 1, 6V supply via VH
         * [6] Stock IR BeconSensor -> State acquired over Create serial line
         * [7] Digital Compass(Honeywell HMC6352)
         *		 I2C line, D3 Clock, D2 Data
         */
        bot.addSensor(new BumpSensor(bot.getCreate()));
        //Front Rangefinder, immobile
        bot.addSensor(new RangeFinder(bot.getSpotBoard().getScalarInputs()[EDemoBoard.A0]));
        //Left Rangefinder, actuated by servo@2
        bot.addSensor(new RangeFinder(bot.getSpotBoard().getScalarInputs()[EDemoBoard.A1]));
        //Servo for left Rangefinder
        bot.addSensor(new SServo(bot.getSpotBoard().getOutputPins()[EDemoBoard.H0], 45.0));
        //Right Rangefinder, actuated by servo@4
        bot.addSensor(new RangeFinder(bot.getSpotBoard().getScalarInputs()[EDemoBoard.A2]));
        //Servo for right RangeFinder
        bot.addSensor(new SServo(bot.getSpotBoard().getOutputPins()[EDemoBoard.H1], 45.0));
        //Beacon Sensor
        bot.addSensor(new BeaconSensor(bot.getCreate()));
        //Compass [RED SDK required]
        bot.addSensor(new Compass(/*bot.getSpotBoard().getI2C()*/null));

        actors = new Vector();

        //LED status YELLOW = ROBOT INITIALIZED
        overallStatus.setColor(LEDColor.YELLOW);

        //Supply actor(s) to manage execution
//        addActor(new CrashActor());
//        addActor(new BeaconActor());
        addActor(new LeftHugger());
//        addActor(new AngleActor());

        bot.setVelocity(VELOCITY);

        //LED status CYAN = ROBOT READY
        overallStatus.setColor(LEDColor.CYAN);
    }

    /**
     * Advanced control of robot cycle
     */
    public void initAdvancedControl() {
        master = bot.getSpotBoard().getSwitches()[0];
        step = bot.getSpotBoard().getSwitches()[1];
        master.addISwitchListener(this);
        step.addISwitchListener(this);
    }

    public void addActor(Actor a) {
        actors.addElement(a);
    }

    /**
     * Mainloop execution follows a fixed cycle:
     * 0. Calibrate robot turning for the desired speed -- requires human intervention
     * LED COLOR : BLUE
     * ----
     * Autonomous Loop
     * LED COLOR : GREEN
     * 1. Update intrinsic sensors -> push data to Actors
     * 2. Ask actors for desired actuation sequence
     *      -if actors request a movement, they engage movement lock
     *       preventing further actuation calls in this sequence.
     * 3. Clear the movement lock in preparation for the next cycle.
     * ANY exception will cause run to exit, resulting in a SPOT reset.
     */
    public void run() {
        overallStatus.setColor(LEDColor.GREEN);
//        running = true;
        try {
            try {
                //Calibration phase, LED color = BLUE

                EDemoBoard demoBoard = bot.getSpotBoard();

                /*Calibrator turncalib=new Calibrator(bot.getCreate());
                System.out.println("Beginning Calibration");
                turncalib.beginCalibration();
                StartApplication.graphical.setColor(LEDColor.BLUE);
                StartApplication.graphical.startPsilon();;
                while(!turncalib.isCalibrated()){
                this.sleep(250);
                }*/
                bot.setOffset(/*turncalib.getOffset()*/0);
                //Calibration complete
                //System.out.println("Calibration Complete");
            } catch (Exception e) {
                e.printStackTrace();
                System.out.println("Failed Calibration");
            }

            while (true) {
                int x = 0;
                if (running) {
//                    System.out.println()
                    System.out.println("" + x);
                    x++;
                    step();
                }
            }
        } catch (Exception e) {
            e.printStackTrace();

        }
        //KERNEL PANIC
        System.out.println("KERNEL PANIC");
        overallStatus.setColor(LEDColor.RED);
    }

    /**
     * We run a single iteration.
     * @throws InterruptedException
     */
    public void step() throws InterruptedException {
        stepping = true;
        //IRobotCreateEventHandler trial=new IRobotCreateEventHandler();
        //trial.setDebug(true);

        //Poll sensors from Robot HW
        overallStatus.setColor(LEDColor.GREEN);
        System.out.println("Kernel Cycle");
        if (bot.getCreate() != null) {
            bot.getCreate().sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, null);
        }

        System.out.println("Robot Poll");
        try {
            bot.getSensors()[0].update(this);
            bot.getSensors()[6].update(this);
        } catch (IOException e) {
            e.printStackTrace();
            overallStatus.setColor(LEDColor.ORANGE);
        }

        //Call Application act() for cycle
        for (int i = 0; i < actors.size(); i++) {
            overallStatus.setColor(LEDColor.BLUE);
            ((Actor) actors.elementAt(i)).act(bot);
            if (bot.isMoving()) {
                //Lock Robot if action requested
                break;
            }
        }
        System.out.println("Actor Cycle");
        //Unlock Robot after action completed
        System.out.println("done step");
        bot.resetMotion();
        stepping = false;
    }

    public void switchPressed(ISwitch is) {
    }

    public void switchReleased(ISwitch is) {
        if (is.equals(master)) {
            //immediately stop kernel loop
            running = !running;
            stopActuationAfterStep();
            setLEDStatus();
            System.out.println("Toggle Control");
        }
        if (is.equals(step)) {
            try {
                step();
                running = false;
                stopActuationAfterStep();
                setLEDStatus();
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }

            System.out.println("Step Control");
        }
    }

    private void setLEDStatus() {
        if (!running) {
            overallStatus.setColor(LEDColor.ORANGE);
        } else {
            overallStatus.setColor(LEDColor.GREEN);
        }
    }

    /**
     * Polls for step completion, once this occurs, stop actuation.
     */
    private void stopActuationAfterStep() {
        final int POLL_TIME = 100;
        while (true) {
            //continuously check until the execution is complete
            //then stop robot
            if (!stepping) {
                bot.stop();
                break;
            }
            try {
                Thread.sleep(POLL_TIME);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }

    }
}
